/***************************************************************************
 创建者: 华磊
 开始时间: 2019.11.27
 copyright: (C) 华友高科
 ***************************************************************************/

#include "zerocalculate.h"
#include "coordinatemanager.h"
#include <QDebug>

ZeroCalculate::ZeroCalculate(CoordinateManager *coordinateManagerIn, RobotCommonParameter *roboParameterIn)
{
    coordinateManager=coordinateManagerIn;
    roboParameter=roboParameterIn;
    searchRange=3;
    iterationsForZero=100;
}

int ZeroCalculate::calculateZeroOffset(std::vector<PointPro> pointList,
                                       std::vector<double> &zeroOffsetOut, double &maxErrorOut)
{
    //只校准关节轴，直线轴不处理。第一个关节轴和最后一个关节轴也不处理。
    //在给定的范围内搜索误差最小的零点。
    //先逐个校准一遍所有轴，然后第二遍，循环ｎ次，使位置偏差最小。
    //第一个设定工具坐标，然后校准零点，再设定工具坐标，再校准零点，循环往复，精度会更加高。
    std::vector<double> tmpZeroOffset;
    for(int i=0;i<iterationsForZero;i++)
    {
        calculateZeroOffset_allAxis(pointList,tmpZeroOffset,tmpZeroOffset,maxErrorOut);
    }
    zeroOffsetOut=tmpZeroOffset;
    return 1;

}

int ZeroCalculate::calculateZeroOffset_oneAxis(int axisId, std::vector<PointPro> pointList,
                                               std::vector<double> zeroOffsetIn, std::vector<double> &zeroOffsetOut, double &maxErrorOut)
{
    zeroOffsetOut=zeroOffsetIn;
    bool isNeedCalculate=true;
    //只校准关节轴，直线轴不处理。第一个关节轴和最后一个关节轴也不处理。
    switch (roboParameter->robotType) {
    case ENUM_SCARA:
        if(0==axisId || 2==axisId || 3==axisId)
        {
            isNeedCalculate=false;
        }
        break;
    case ENUM_6AXIS_CHAIN:
        if(0==axisId || 5==axisId)
        {
            isNeedCalculate=false;
        }
        break;
    case ENUM_DESCART_WRAC_ROBOT:
    case ENUM_DESCART_WRACTEST_ROBOT:
    case ENUM_DESCART_WRIST_ROBOT:
        if(0==axisId || 1==axisId || 2==axisId || 3==axisId || 5==axisId)
        {
            isNeedCalculate=false;
        }
        break;
    default:
    {
        qDebug()<<"error,,,ZeroCalculate::calculateZeroOffset_allAxis unsupport robotType";
        return -1;
    }
    }
    if(false==isNeedCalculate)
    {
        return 0;
    }

    //在给定的范围内搜索误差最小的零点。使用中间值搜索法。按照迭代次数或误差小来结束迭代。
    double tmpMaxError;
    calculateMaxError(pointList,zeroOffsetOut,tmpMaxError);
    int maxIteration_oneAxis=100;
    double errorDiffCheck=0.1;
    bool isMoveForward=true;//尝试方向
    double currentError;
    for(int i=0;i<maxIteration_oneAxis;i++)
    {
        if(isMoveForward)
        {
            zeroOffsetOut[axisId]=(zeroOffsetOut[axisId]+searchRange)/2.0;
        }
        else
        {
            zeroOffsetOut[axisId]=(zeroOffsetOut[axisId]-searchRange)/2.0;
        }
        calculateMaxError(pointList,zeroOffsetOut,currentError);
        if(currentError>tmpMaxError)
        {
            isMoveForward=!isMoveForward;
        }
        else
        {
            if(currentError<errorDiffCheck)
            {
                maxErrorOut=currentError;
                qDebug()<<"currentError<errorDiffCheck axisId"<<axisId;
                return 1;
            }

        }
        tmpMaxError=currentError;
    }
    qDebug()<<"maxIteration_oneAxis axisId"<<axisId;
    maxErrorOut=tmpMaxError;
    return 1;

}

int ZeroCalculate::calculateZeroOffset_allAxis(std::vector<PointPro> pointList,
                                    std::vector<double> zeroOffsetIn, std::vector<double> &zeroOffsetOut, double &maxErrorOut)
{

    switch (roboParameter->robotType) {
    case ENUM_SCARA:
        if(zeroOffsetIn.size()!=4)
        {
            zeroOffsetIn.clear();
            zeroOffsetIn.resize(4);
            zeroOffsetIn[0]=0;
            zeroOffsetIn[1]=0;
            zeroOffsetIn[2]=0;
            zeroOffsetIn[3]=0;
        }
        break;
    case ENUM_6AXIS_CHAIN:
        if(zeroOffsetIn.size()!=6)
        {
            zeroOffsetIn.clear();
            zeroOffsetIn.resize(6);
            zeroOffsetIn[0]=0;
            zeroOffsetIn[1]=0;
            zeroOffsetIn[2]=0;
            zeroOffsetIn[3]=0;
            zeroOffsetIn[5]=0;
        }
        break;
    default:
    {
        qDebug()<<"error,,,ZeroCalculate::calculateZeroOffset_allAxis unsupport robotType";
        return -1;
    }
    }



    for(int i=0;i<zeroOffsetIn.size();i++)
    {
        calculateZeroOffset_oneAxis(i,pointList,zeroOffsetIn,zeroOffsetIn,maxErrorOut);
    }
    zeroOffsetOut=zeroOffsetIn;
    return 1;

}

int ZeroCalculate::calculateMaxError(std::vector<PointPro> pointList, std::vector<double> zeroOffsetIn, double &maxErrorOut)
{
    std::vector<PointPro> pointListOut=pointList;
    //叠加偏差
    for(int i=0;i<pointList.size();i++)
    {
        if(JOINT_FLAG_CARTESIAN==pointList[i].jointFlag)
        {
            qDebug()<<"error,,,JOINT_FLAG_CARTESIAN==pointList[i].jointFlag";

        }
        for(int j=0;j<pointList[i].positionValue.size();j++)
        {
            pointList[i].positionValue[j]+=zeroOffsetIn[j];
        }
    }
    //求坐标点
    for(int i=0;i<pointList.size();i++)
    {
        coordinateManager->pointCoordinateToCartesian(pointList[i],pointListOut[i]);
    }
    //比较最大值
    maxErrorOut=0;
    for(int i=0;i<pointListOut.size();i++)
    {
        for(int j=i+1;j<pointListOut.size();j++)
        {
            double tmpCurrentError=sqrt((pointListOut[i].positionValue[0]-pointListOut[j].positionValue[0])
                    *(pointListOut[i].positionValue[0]-pointListOut[j].positionValue[0])
                    +(pointListOut[i].positionValue[1]-pointListOut[j].positionValue[1])
                                        *(pointListOut[i].positionValue[1]-pointListOut[j].positionValue[1])
                    +(pointListOut[i].positionValue[2]-pointListOut[j].positionValue[2])
                                        *(pointListOut[i].positionValue[2]-pointListOut[j].positionValue[2]));
            if(tmpCurrentError>maxErrorOut)
            {
                maxErrorOut=tmpCurrentError;
            }

        }
    }
    return 1;

}
